/*
 *  MotorController_test.cpp
 *
 *  Created by: Kellen Carey
 *  Date: 11/14/2013
 */

#include "MotorController.h"
#include <iostream>
#include <string>
#include <time.h>

using namespace std;

int main(int argc, char* argv[]) {
	MotorController motorController;
	motorController.setMotion(STAND);
	motorController.step(false);

	string input;
	bool done = false;

	do {
		cout << " Enter \"W\" for Walk\n \"Q\" for Left turn\n \"E\" for Right turn\n \"S\" for Stand\n \"K\" for kick\n \"X\" will exit. ";

		cin >> input;

		if (input == "W")
		{
			motorController.setMotion(WALK);
			cout << "Walking" << endl;
		}
		else if (input == "Q")
		{
			motorController.setMotion(TURNLEFT);
			cout << "Turning Left" << endl;
		}
		else if (input == "E")
		{
			motorController.setMotion(TURNRIGHT);
			cout << "Turning Right" << endl;
		}
		else if (input == "S")
		{
			motorController.setMotion(STAND);
			cout << "Standing" << endl;
		}
//		else if (input == "A")
//		{
//			motorController.setMotion(SIDESTEPLEFT);
//			cout << "Stepping Left" << endl;
//		}
//		else if (input == "D")
//		{
//			motorController.setMotion(SIDESTEPRIGHT);
//			cout << "Stepping Right" << endl;
//		}
		else if (input == "K")
		{
			motorController.setMotion(KICK);
			cout << "Kicking (left foot)" << endl;
		}
		else if (input == "X"){
			done = true;
		}
		else{
			cout << "Please enter a valid input" << endl;
		}
		clock_t startTime = clock();

		while ((((float)clock() - (float)startTime)/CLOCKS_PER_SEC) < 10 && !done)
		{
			motorController.step(false);
		}

	} while (!done);

	return 0;
}
